#include "mcmcLowLevelStat4Affinity.h"
#include "mcmcImageFilters.h"

///////////////////////////////////////////////
///////////////////////////////////////////////
// construction & destruction
MCMC_LowLevelStat4Region::MCMC_LowLevelStat4Region()
{
	p_histogram_obs = NULL;
	num_his = 0;
}

MCMC_LowLevelStat4Region::~MCMC_LowLevelStat4Region()
{
	Free();
}

void MCMC_LowLevelStat4Region::Free(void)
{
	if (p_histogram_obs != NULL)
		delete []p_histogram_obs;
	p_histogram_obs = NULL;

	num_his = 0;
}

MCMC_LowLevelStat4Region & MCMC_LowLevelStat4Region::operator=(const MCMC_LowLevelStat4Region &aa)
{
	if (this != &aa)
	{
		Free();

		SetNumFilters(aa.num_his);

		for (int i=0; i<num_his; i++)
			p_histogram_obs[i] = aa.p_histogram_obs[i];

		his_intensity = aa.his_intensity;

		mean = aa.mean;
		var  = aa.var;
		num_pixel = aa.num_pixel;

		a = aa.a;
		b = aa.b;
		c = aa.c;
	}

	return *this;
}


// operations
void MCMC_LowLevelStat4Region::SetNumFilters(const int num)
{
	if (num_his != num)
	{
		Free();

		num_his = num;

		if (num > 0)
			p_histogram_obs = new Histogram[num];
	}
}

void MCMC_LowLevelStat4Region::Clear(void)
{
	for (int i=0; i<num_his; i++)
		p_histogram_obs[i].Init(0.0);

	num_pixel = 0;
	mean = 0.0;
	var = 0.0;

	a = 0.0;
	b = 0.0;
	c = 0.0;
}


void MCMC_LowLevelStat4Region::Update(MCMC_FilterResp &resp, MCMC_PixelSet &pixel_set)
{
	MCMC_FilterBank	*pfilter_bank;
	Raster<float>	*pras_intensity;
	
	// the filter bank on the intensity image
	pfilter_bank = &resp.filter_bank_intensity;
	pras_intensity = &resp.ras_intensity;
	
	SetNumFilters(pfilter_bank->NumFilter());
	Clear();

	int		k,i,j;;
	bool	bgo;
	
	his_intensity.Set(INTENSITY_LOW,INTENSITY_HIGH,INTENSITY_DELTA);
	// filtering results
	for (k=0; k<pfilter_bank->NumFilter(); k++)
		p_histogram_obs[k].Set(pfilter_bank->p_min_value[k],pfilter_bank->p_max_value[k], pfilter_bank->p_deltas[k]);

	bgo = pixel_set.GetFirst(j, i);
	while (bgo)
	{
		// mean and variance
		if (pras_intensity->Valid(j, i))
		{
			for (k=0; k<pfilter_bank->NumFilter(); k++)
				p_histogram_obs[k].AddBin(pfilter_bank->p_filtering_result[k](j,i),1.0);

			his_intensity.AddBin(pras_intensity->Data(j,i), 1.0);
			mean += pras_intensity->Data(j,i);

			num_pixel++;
		}
		
		bgo = pixel_set.GetNext(j, i);
	}
	
	// normalize the histograms
	his_intensity.Normalize();
	for (k=0; k<pfilter_bank->NumFilter(); k++)
		p_histogram_obs[k].Normalize();

	double			dtemp;
	mcmcMatrix<double>	mx_Y,mx_A,mx_AT,mx_X;

	mx_Y.SetDimension(num_pixel, 1);
	mx_A.SetDimension(num_pixel, 3);

	int t;
	// compute variance
	if (num_pixel > 0)
	{
		// mean
		mean /= num_pixel;
		
		t = 0;
		bgo = pixel_set.GetFirst(j, i);
		while (bgo)
		{
			if (pras_intensity->Valid(j, i))
			{
				dtemp = pras_intensity->Data(j, i)-mean;
				var += dtemp*dtemp;

				mx_Y(t) = pras_intensity->Data(j, i);
				mx_A(t, 0) = j;
				mx_A(t, 1) = i;
				mx_A(t, 2) = 1;

				t++;
			}

			bgo = pixel_set.GetNext(j, i);
		}
		var /=  num_pixel;
	}
	
	// fit the image with a plane using the least-square method
	if (num_pixel > 4)
	{
		mx_AT = mx_A.T();
		mx_A = mx_AT*mx_A;
		mx_X = mx_A.Inv()*mx_AT*mx_Y;

		a = mx_X(0);
		b = mx_X(1);
		c = mx_X(2);
	}
}
